using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SilkSharp.src
{
	/*
	 *  NLSF stabilizer:
	 *  - Moves NLSFs further apart if they are too close                                                   
	 *  - Moves NLSFs away from borders if they are too close    
	 *	- High effort to achieve a modification with minimum Euclidean distance to input vector                   
	 *	- Output are sorted NLSF coefficients   
	 *  
	 * @author Jing Dai
	 * @author Dingxin Xu
	 */
	public class Silk_NLSF_stabilize
	{
		/*
		 * Constant Definitions.
		 */
		public const int MAX_LOOPS = 20;

		/*
		 * NLSF stabilizer, for a single input data vector.
		 * @param NLSF_Q15 Unstable/stabilized normalized LSF vector in Q15 [L].
		 * @param NLSF_Q15_offset offset of valid data.
		 * @param NDeltaMin_Q15 Normalized delta min vector in Q15, NDeltaMin_Q15[L] must be >= 1 [L+1].
		 * @param L Number of NLSF parameters in the input vector.
		 */
		public static void SKP_Silk_NLSF_stabilize(
				  int[] NLSF_Q15,            /* I/O:  Unstable/stabilized normalized LSF vector in Q15 [L]                    */
				  int NLSF_Q15_offset,
				  int[] NDeltaMin_Q15,       /* I:    Normalized delta min vector in Q15, NDeltaMin_Q15[L] must be >= 1 [L+1] */
		   int L                    /* I:    Number of NLSF parameters in the input vector                           */
		)
		{
			int center_freq_Q15, diff_Q15, min_center_Q15, max_center_Q15;
			int min_diff_Q15;
			int loops;
			int i, I = 0, k;

			/* This is necessary to ensure an output within range of a SKP_int16 */
			Silk_typedef.SKP_assert( NDeltaMin_Q15[ L ] >= 1 );

			for( loops = 0; loops < MAX_LOOPS; loops++ )
			{
				/**************************/
				/* Find smallest distance */
				/**************************/
				/* First element */
				min_diff_Q15 = NLSF_Q15[ NLSF_Q15_offset + 0 ] - NDeltaMin_Q15[ 0 ];
				I = 0;
				/* Middle elements */
				for( i = 1; i <= L - 1; i++ )
				{
					diff_Q15 = NLSF_Q15[ NLSF_Q15_offset + i ] - ( NLSF_Q15[ NLSF_Q15_offset + i - 1 ] + NDeltaMin_Q15[ i ] );
					if( diff_Q15 < min_diff_Q15 )
					{
						min_diff_Q15 = diff_Q15;
						I = i;
					}
				}
				/* Last element */
				diff_Q15 = ( 1 << 15 ) - ( NLSF_Q15[ NLSF_Q15_offset + L - 1 ] + NDeltaMin_Q15[ L ] );
				if( diff_Q15 < min_diff_Q15 )
				{
					min_diff_Q15 = diff_Q15;
					I = L;
				}

				/***************************************************/
				/* Now check if the smallest distance non-negative */
				/***************************************************/
				if( min_diff_Q15 >= 0 )
				{
					return;
				}

				if( I == 0 )
				{
					/* Move away from lower limit */
					NLSF_Q15[ NLSF_Q15_offset + 0 ] = NDeltaMin_Q15[ 0 ];

				}
				else if( I == L )
				{
					/* Move away from higher limit */
					NLSF_Q15[ NLSF_Q15_offset + L - 1 ] = ( 1 << 15 ) - NDeltaMin_Q15[ L ];

				}
				else
				{
					/* Find the lower extreme for the location of the current center frequency */
					min_center_Q15 = 0;
					for( k = 0; k < I; k++ )
					{
						min_center_Q15 += NDeltaMin_Q15[ k ];
					}
					min_center_Q15 += ( NDeltaMin_Q15[ I ] >> 1 );


					/* Find the upper extreme for the location of the current center frequency */
					max_center_Q15 = ( 1 << 15 );
					for( k = L; k > I; k-- )
					{
						max_center_Q15 -= NDeltaMin_Q15[ k ];
					}
					max_center_Q15 -= ( NDeltaMin_Q15[ I ] - ( NDeltaMin_Q15[ I ] >> 1 ) );

					/* Move apart, sorted by value, keeping the same center frequency */
					center_freq_Q15 = Silk_SigProc_FIX.SKP_LIMIT_32( Silk_SigProc_FIX.SKP_RSHIFT_ROUND( (int)NLSF_Q15[ NLSF_Q15_offset + I - 1 ] + (int)NLSF_Q15[ NLSF_Q15_offset + I ], 1 ),
							min_center_Q15, max_center_Q15 );
					NLSF_Q15[ NLSF_Q15_offset + I - 1 ] = center_freq_Q15 - ( NDeltaMin_Q15[ I ] >> 1 );
					NLSF_Q15[ NLSF_Q15_offset + I ] = NLSF_Q15[ NLSF_Q15_offset + I - 1 ] + NDeltaMin_Q15[ I ];
				}
			}

			/* Safe and simple fall back method, which is less ideal than the above */
			if( loops == MAX_LOOPS )
			{
				/* Insertion sort (fast for already almost sorted arrays):   */
				/* Best case:  O(n)   for an already sorted array            */
				/* Worst case: O(n^2) for an inversely sorted array          */
				Silk_sort.SKP_Silk_insertion_sort_increasing_all_values( NLSF_Q15, NLSF_Q15_offset + 0, L );

				/* First NLSF should be no less than NDeltaMin[0] */
				NLSF_Q15[ NLSF_Q15_offset + 0 ] = Silk_SigProc_FIX.SKP_max_int( NLSF_Q15[ NLSF_Q15_offset + 0 ], NDeltaMin_Q15[ 0 ] );

				/* Keep delta_min distance between the NLSFs */
				for( i = 1; i < L; i++ )
					NLSF_Q15[ NLSF_Q15_offset + i ] = Silk_SigProc_FIX.SKP_max_int( NLSF_Q15[ NLSF_Q15_offset + i ], NLSF_Q15[ NLSF_Q15_offset + i - 1 ] + NDeltaMin_Q15[ i ] );

				/* Last NLSF should be no higher than 1 - NDeltaMin[L] */
				NLSF_Q15[ NLSF_Q15_offset + L - 1 ] = Silk_SigProc_FIX.SKP_min_int( NLSF_Q15[ NLSF_Q15_offset + L - 1 ], ( 1 << 15 ) - NDeltaMin_Q15[ L ] );

				/* Keep NDeltaMin distance between the NLSFs */
				for( i = L - 2; i >= 0; i-- )
					NLSF_Q15[ NLSF_Q15_offset + i ] = Silk_SigProc_FIX.SKP_min_int( NLSF_Q15[ NLSF_Q15_offset + i ], NLSF_Q15[ NLSF_Q15_offset + i + 1 ] - NDeltaMin_Q15[ i + 1 ] );
			}
		}

		/*
		 * NLSF stabilizer, over multiple input column data vectors.
		 * @param NLSF_Q15 Unstable/stabilized normalized LSF vectors in Q15 [LxN].
		 * @param NDeltaMin_Q15 Normalized delta min vector in Q15, NDeltaMin_Q15[L] must be >= 1 [L+1].
		 * @param N Number of input vectors to be stabilized.
		 * @param L NLSF vector dimension.
		 */
		public static void SKP_Silk_NLSF_stabilize_multi(
			int[] NLSF_Q15,        /* I/O:  Unstable/stabilized normalized LSF vectors in Q15 [LxN]                 */
			int[] NDeltaMin_Q15,   /* I:    Normalized delta min vector in Q15, NDeltaMin_Q15[L] must be >= 1 [L+1] */
			int N,               /* I:    Number of input vectors to be stabilized                                */
			int L                /* I:    NLSF vector dimension                                                   */
		)
		{
			int n;

			/* loop over input data */
			for( n = 0; n < N; n++ )
			{
				SKP_Silk_NLSF_stabilize( NLSF_Q15, n * L, NDeltaMin_Q15, L );
			}
		}
	}

}
